/*
 * ESP32 Native PWM Servo Control Test Program
 * Using ESP32's built-in LEDC PWM functionality
 * For MG90S servo motors
 * 
 * Hardware Connections:
 * Servo X (Pan):  Signal -> GPIO 18, VCC -> 5V, GND -> GND
 * Servo Y (Tilt): Signal -> GPIO 19, VCC -> 5V, GND -> GND
 * 
 * IMPORTANT: Use external 5V power supply for servos!
 * ESP32 3.3V is NOT sufficient for servo operation.
 */

// PWM Configuration
#define PWM_FREQ 50        // 50Hz for servo control
#define PWM_RESOLUTION 16  // 16-bit resolution (0-65535)
#define PWM_CHANNEL_X 0    // PWM channel for X servo
#define PWM_CHANNEL_Y 1    // PWM channel for Y servo

// Servo pins
#define SERVO_X_PIN 18
#define SERVO_Y_PIN 19

// PWM timing parameters (microseconds)
struct PWMParams {
  int minPulse;
  int maxPulse;
  int centerPulse;
  const char* name;
};

// Different parameter sets to test
PWMParams paramSets[] = {
  {500, 2400, 1450, "标准MG90S参数"},
  {600, 2400, 1500, "保守范围参数"},
  {544, 2400, 1472, "Arduino标准参数"},
  {1000, 2000, 1500, "窄范围参数"},
  {400, 2600, 1500, "宽范围参数"},
  {700, 2300, 1500, "安全范围参数"}
};

int currentParamSet = 0;
int totalParamSets = sizeof(paramSets) / sizeof(paramSets[0]);
bool testRunning = false;
int testStep = 0;
unsigned long lastStepTime = 0;
const int stepDelay = 2000; // 2 seconds between steps

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("\n=== ESP32原生PWM舵机控制测试程序 ===");
  Serial.println("系统性测试不同PWM参数组合");
  Serial.println("控制命令:");
  Serial.println("  'n' - 下一个参数组合");
  Serial.println("  'p' - 上一个参数组合");
  Serial.println("  't' - 开始/停止测试序列");
  Serial.println("  'r' - 重置到第一个参数组合");
  Serial.println("  '0' - 移动到0度");
  Serial.println("  '9' - 移动到90度");
  Serial.println("  '1' - 移动到180度");
  Serial.println("  's' - 显示当前参数");
  Serial.println("================================================\n");
  
  // Configure PWM channels
  setupPWM();
  
  // Show initial parameters
  showCurrentParams();
  
  // Move to center position
  moveToCenter();
}

void setupPWM() {
  // Configure PWM channels (new ESP32 Arduino core API)
  ledcAttach(SERVO_X_PIN, PWM_FREQ, PWM_RESOLUTION);
  ledcAttach(SERVO_Y_PIN, PWM_FREQ, PWM_RESOLUTION);
  
  Serial.println("PWM引脚配置完成:");
  Serial.printf("  引脚 %d (舵机X)\n", SERVO_X_PIN);
  Serial.printf("  引脚 %d (舵机Y)\n", SERVO_Y_PIN);
  Serial.printf("  频率: %d Hz\n", PWM_FREQ);
  Serial.printf("  分辨率: %d 位\n", PWM_RESOLUTION);
}

void showCurrentParams() {
  PWMParams &params = paramSets[currentParamSet];
  Serial.printf("\n=== 当前参数组合 %d/%d ===\n", currentParamSet + 1, totalParamSets);
  Serial.printf("名称: %s\n", params.name);
  Serial.printf("最小脉宽: %d μs\n", params.minPulse);
  Serial.printf("最大脉宽: %d μs\n", params.maxPulse);
  Serial.printf("中心脉宽: %d μs\n", params.centerPulse);
  
  // Calculate duty cycles
  int minDuty = microsecondsToDutyCycle(params.minPulse);
  int maxDuty = microsecondsToDutyCycle(params.maxPulse);
  int centerDuty = microsecondsToDutyCycle(params.centerPulse);
  
  Serial.printf("占空比数值: 最小=%d, 中心=%d, 最大=%d\n", minDuty, centerDuty, maxDuty);
  Serial.println("=====================================\n");
}

int microsecondsToDutyCycle(int microseconds) {
  // Convert microseconds to duty cycle value
  // PWM period = 1/50Hz = 20ms = 20000μs
  // Duty cycle = (pulse_width / period) * (2^resolution - 1)
  float period = 1000000.0 / PWM_FREQ; // Period in microseconds
  float dutyCycle = (float)microseconds / period * ((1 << PWM_RESOLUTION) - 1);
  return (int)dutyCycle;
}

void setServoAngle(int pin, int angle) {
  PWMParams &params = paramSets[currentParamSet];
  
  // Constrain angle
  angle = constrain(angle, 0, 180);
  
  // Calculate pulse width
  int pulseWidth = map(angle, 0, 180, params.minPulse, params.maxPulse);
  
  // Convert to duty cycle
  int dutyCycle = microsecondsToDutyCycle(pulseWidth);
  
  // Set PWM (new API uses pin number directly)
  ledcWrite(pin, dutyCycle);
  
  Serial.printf("舵机%s: %d° -> %dμs -> 占空比=%d\n", 
                pin == SERVO_X_PIN ? "X" : "Y", 
                angle, pulseWidth, dutyCycle);
}

void moveToCenter() {
  Serial.println("将两个舵机移动到中心位置 (90°)");
  setServoAngle(SERVO_X_PIN, 90);
  setServoAngle(SERVO_Y_PIN, 90);
}

void runTestSequence() {
  if (!testRunning) return;
  
  unsigned long currentTime = millis();
  if (currentTime - lastStepTime < stepDelay) return;
  
  lastStepTime = currentTime;
  
  switch (testStep) {
    case 0:
      Serial.println("\n=== 测试步骤1: 移动到0度 ===");
      setServoAngle(SERVO_X_PIN, 0);
      setServoAngle(SERVO_Y_PIN, 0);
      break;
      
    case 1:
      Serial.println("\n=== 测试步骤2: 移动到90度(中心位置) ===");
      setServoAngle(SERVO_X_PIN, 90);
      setServoAngle(SERVO_Y_PIN, 90);
      break;
      
    case 2:
      Serial.println("\n=== 测试步骤3: 移动到180度 ===");
      setServoAngle(SERVO_X_PIN, 180);
      setServoAngle(SERVO_Y_PIN, 180);
      break;
      
    case 3:
      Serial.println("\n=== 测试步骤4: 回到中心位置 ===");
      setServoAngle(SERVO_X_PIN, 90);
      setServoAngle(SERVO_Y_PIN, 90);
      break;
      
    case 4:
      Serial.println("\n=== 测试步骤5: 摆动测试 ===");
      setServoAngle(SERVO_X_PIN, 45);
      setServoAngle(SERVO_Y_PIN, 135);
      break;
      
    case 5:
      Serial.println("\n=== 测试步骤6: 反向摆动 ===");
      setServoAngle(SERVO_X_PIN, 135);
      setServoAngle(SERVO_Y_PIN, 45);
      break;
      
    case 6:
      Serial.println("\n=== 测试完成! ===");
      moveToCenter();
      testRunning = false;
      testStep = 0;
      Serial.println("舵机在这个参数下是否正常工作?");
      Serial.println("发送 'n' 测试下一个参数组合，或 't' 重复测试\n");
      return;
  }
  
  testStep++;
}

void handleSerialInput() {
  if (!Serial.available()) return;
  
  char command = Serial.read();
  
  switch (command) {
    case 'n':
    case 'N':
      currentParamSet = (currentParamSet + 1) % totalParamSets;
      showCurrentParams();
      moveToCenter();
      break;
      
    case 'p':
    case 'P':
      currentParamSet = (currentParamSet - 1 + totalParamSets) % totalParamSets;
      showCurrentParams();
      moveToCenter();
      break;
      
    case 't':
    case 'T':
      testRunning = !testRunning;
      if (testRunning) {
        Serial.println("\n*** 开始测试序列 ***");
        testStep = 0;
        lastStepTime = millis();
      } else {
        Serial.println("\n*** 测试序列已停止 ***");
        moveToCenter();
      }
      break;
      
    case 'r':
    case 'R':
      currentParamSet = 0;
      testRunning = false;
      testStep = 0;
      showCurrentParams();
      moveToCenter();
      break;
      
    case '0':
      Serial.println("手动控制: 移动到0度");
      setServoAngle(SERVO_X_PIN, 0);
      setServoAngle(SERVO_Y_PIN, 0);
      break;
      
    case '9':
      Serial.println("手动控制: 移动到90度");
      setServoAngle(SERVO_X_PIN, 90);
      setServoAngle(SERVO_Y_PIN, 90);
      break;
      
    case '1':
      Serial.println("手动控制: 移动到180度");
      setServoAngle(SERVO_X_PIN, 180);
      setServoAngle(SERVO_Y_PIN, 180);
      break;
      
    case 's':
    case 'S':
      showCurrentParams();
      break;
      
    default:
      Serial.println("未知命令。可用命令:");
      Serial.println("  'n' - 下一个参数组合");
      Serial.println("  'p' - 上一个参数组合");
      Serial.println("  't' - 开始/停止测试序列");
      Serial.println("  'r' - 重置到第一个参数组合");
      Serial.println("  '0' - 移动到0度");
      Serial.println("  '9' - 移动到90度");
      Serial.println("  '1' - 移动到180度");
      Serial.println("  's' - 显示当前参数");
      break;
  }
}

void loop() {
  handleSerialInput();
  runTestSequence();
  delay(10);
}

/*
 * Troubleshooting Guide:
 * 
 * 1. No servo movement:
 *    - Check 5V external power supply
 *    - Verify signal wire connections
 *    - Try different parameter sets
 *    - Check if servo is damaged
 * 
 * 2. Servo jitters or moves incorrectly:
 *    - Try different PWM parameters
 *    - Check power supply current capacity
 *    - Verify ground connections
 * 
 * 3. Limited range of motion:
 *    - Adjust minPulse and maxPulse values
 *    - Some servos have different specifications
 * 
 * 4. Servo moves but not to correct angles:
 *    - Calibrate centerPulse value
 *    - Adjust the pulse width range
 * 
 * Expected behavior:
 * - Servos should move smoothly to each position
 * - No jittering or unusual sounds
 * - Full 180-degree range of motion
 * - Accurate positioning
 */